`timescale 1ns / 1ns

module sin_cos (clk,angle,sin,cos);

input clk;
input signed [31:0] angle;

output signed [31:0] sin;
output signed [31:0] cos;

integer i;

parameter PIPELINE = 20;
parameter KN = 40'd39797;

wire [31:0] rot[PIPELINE-1:0];

assign  rot[0] = 32'd2949120,
        rot[1] = 32'd1740970,
        rot[2] = 32'd919876,
        rot[3] = 32'd466944,
        rot[4] = 32'd234376,
        rot[5] = 32'd117303,
        rot[6] = 32'd58668,
        rot[7] = 32'd29334,
        rot[8] = 32'd14667,
        rot[9] = 32'd7333,
        rot[10] = 32'd3670,
        rot[11] = 32'd1835,
        rot[12] = 32'd918,
        rot[13] = 32'd459,
        rot[14] = 32'd229,
        rot[15] = 32'd118,
        rot[16] = 32'd57,
        rot[17] = 32'd29,
        rot[18] = 32'd14,
        rot[19] = 32'd7;

reg signed [31:0]   x[PIPELINE-1:0],
                    y[PIPELINE-1:0],
                    z[PIPELINE-1:0];
reg [1:0] quat[PIPELINE-1:0];

assign cos = (quat[0] ^ quat[1] == 1'b0 ? x[PIPELINE-1] : -x[PIPELINE-1]) * KN >>> 20;
assign sin = (quat[1] == 1'b0 ? y[PIPELINE-1] : -y[PIPELINE-1]) * KN >>> 20;

always @(posedge clk) begin
    x[0] <= 32'h100000;
    y[0] <= 32'h0;
    z[0] <= angle % 32'd5898240;
    quat[0] <= angle / 32'd5898240;
end

always @(posedge clk) begin
    for (i = 1; i < PIPELINE; i++) begin
        if (z[i-1] > 0) begin
            z[i] <= z[i-1] - rot[i-1];
            x[i] <= x[i-1] - (y[i-1] >>> (i-1));
            y[i] <= y[i-1] + (x[i-1] >>> (i-1));
        end
        else begin
            z[i] <= z[i-1] + rot[i-1];
            x[i] <= x[i-1] + (y[i-1] >>> (i-1));
            y[i] <= y[i-1] - (x[i-1] >>> (i-1));
        end
        quat[i] <= quat[i-1];
    end
end

endmodule